/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Author : Dula Nad
 *  Created: 01.02.2013.
 *********************************************************************/
#include "BackwardCompatibility.hpp"
#include "../ConfigXMLParser.h"
#include "../ROSInterface.h"

#include <osg/MatrixTransform>
#include <osg/PositionAttitudeTransform>

PP_LABUST_MAKE_SENSORFACTORY_EXT(VCamSensorFactory,
		VirtualCamera,
		Vcam,
		VirtualCameraToROSImage,
		&ConfigFile::processVcam,
		"virtualCameraGen",
		"VirtualCameraGenToROSImage",
		(hook->root)(config->name)
		(hook->vMs)
		(config->resw)(config->resh)
		(config->baseLine)(config->frameId)
		(config->parameters.get())(0),
		(sensor.get())
		(rosInterface.topic)
		(rosInterface.infoTopic)
		(rosInterface.rate)(0))

class VCamSensorFactoryDetail : public virtual VCamSensorFactory
{
public:
	VCamSensorFactoryDetail():Base(&ConfigFile::processVcam,"virtualCameraGen",
			"VirtualCameraGenToROSImage"){};

	void createSensor(plugin::UWSimHook::Ptr hook)
	{
		hook->urdf->link[config->link]->asGroup()->addChild(hook->vMs);
		sensor.reset(new VirtualCamera(hook->root, config->name, hook->vMs, config->resw, config->resh, config->baseLine, config->frameId, config->parameters.get(), 0));

		sensor->textureCamera->addChild(hook->scene->getScene());
		hook->root->addChild(sensor->textureCamera);

		//This could be passed also with the hook.
		/*osg::Fog *fog = new osg::Fog();
				    	fog->setMode(osg::Fog::EXP2);
				    	fog->setFogCoordinateSource(osg::Fog::FRAGMENT_DEPTH);
				    	//fog->setDensity(confFile.fogDensity);
				    	//fog->setColor(osg::Vec4d(confFile.fogColor[0],confFile.fogColor[1],confFile.fogColor[2],1));
				    	sensor->textureCamera->getOrCreateStateSet()->setAttributeAndModes(fog, osg::StateAttribute::ON);*/

		if (config->showpath) sensor->showPath(config->showpath);
	}
};

PP_LABUST_MAKE_SENSORFACTORY_EXT(VRangeImageSensorFactory,
		VirtualCamera,
		Vcam,
		VirtualCameraToROSImage,
		&ConfigFile::processVcam,
		"virtualCameraGen",
		"VirtualCameraGenToROSImage",
		(hook->root)(config->name)
		(hook->vMs)
		(config->resw)(config->resh)
		(config->baseLine)(config->frameId)
		(config->parameters.get())(1),
		(sensor.get())
		(rosInterface.topic)
		(rosInterface.infoTopic)
		(rosInterface.rate)(1))


class VRangeImageSensorFactoryDetail : public virtual VRangeImageSensorFactory
{
public:
	 VRangeImageSensorFactoryDetail():Base(&ConfigFile::processVcam,"virtualRangeImageGen",
			"RangeImageSensorGenToROSImage"){};

		void createSensor(plugin::UWSimHook::Ptr hook)
		{
			hook->urdf->link[config->link]->asGroup()->addChild(hook->vMs);
			sensor.reset(new VirtualCamera(hook->root, config->name, hook->vMs, config->resw, config->resh, config->baseLine, config->frameId, config->parameters.get(), 1));

			sensor->textureCamera->addChild(hook->scene->getScene());
			hook->root->addChild(sensor->textureCamera);

			//This could be passed also with the hook.
			/*osg::Fog *fog = new osg::Fog();
					    	fog->setMode(osg::Fog::EXP2);
					    	fog->setFogCoordinateSource(osg::Fog::FRAGMENT_DEPTH);
					    	//fog->setDensity(confFile.fogDensity);
					    	//fog->setColor(osg::Vec4d(confFile.fogColor[0],confFile.fogColor[1],confFile.fogColor[2],1));
					    	sensor->textureCamera->getOrCreateStateSet()->setAttributeAndModes(fog, osg::StateAttribute::ON);*/

			if (config->showpath) sensor->showPath(config->showpath);
		}
};

extern "C"
{
//The first draft of the actual sensor collection exported function in a plugin.
void registerSensors(plugin::SensorList& list, plugin::ROSInterfaceList& rlist)
{
	list.push_back(plugin::SensorInterface::Ptr(new VCamSensorFactoryDetail()));
	list.push_back(plugin::SensorInterface::Ptr(new VRangeImageSensorFactoryDetail()));
}
};
